#coding=utf-8

#   Qt界面
from PySide2 import QtCore
from PySide2.QtCore import QTimer
from PySide2.QtWidgets import QApplication, QSlider,QLCDNumber,QRadioButton,QStatusBar,QLabel
from PySide2.QtUiTools import QUiLoader
#   自定义控件
from gui.widget3d import env_robotModel
from gui.widget import battery, accelWidget,pressureWidget,zmpWidget,videoWidget,logWidget
from gui import jctrl_panel, bctrl_panel, params_panel
from proc_data.recorder import *
from controller.params import Params
class Panel:
    def __init__(self,rootObject=None,jointNums=6):
        '''
        :param rootObject:  上层实例
        '''
        self.rootObject=rootObject
        self.jointNums = jointNums
        self.ui_Main=QUiLoader().load("gui/qt_ui_design/dual_leg_panel.ui")

    def ConfigPanel(self):
        #   添加数据可视化Widget
        #   压力显示
        self.pressureL=pressureWidget.PressureWidget(title="左脚压力曲线",freq=Params.controlParams['freq_high'])
        self.ui_Main.pressurelayout.addWidget(self.pressureL)
        self.pressureR = pressureWidget.PressureWidget(title="右脚压力曲线",freq=Params.controlParams['freq_high'])
        self.ui_Main.pressurelayout.addWidget(self.pressureR)
        #   ZMP显示
        self.ZMPL=zmpWidget.ZMPWidget(title='左脚ZMP',freq=Params.controlParams['freq_high'])
        self.ZMPR = zmpWidget.ZMPWidget(title='右脚ZMP', freq=Params.controlParams['freq_high'])
        self.ui_Main.zmpLayout.addWidget(self.ZMPL)
        self.ui_Main.zmpLayout.addWidget(self.ZMPR)
        #   姿态显示
        self.rollCanvas= accelWidget.AccelWidget(title="Roll数值曲线",freq=Params.controlParams['freq_high'])
        self.ui_Main.gyroLayout.addWidget(self.rollCanvas)
        self.pitchCanvas = accelWidget.AccelWidget(title="Pitch数值曲线",freq=Params.controlParams['freq_high'])
        self.ui_Main.gyroLayout.addWidget(self.pitchCanvas)

        #   机器人姿态与骨骼点s
        self.modelWidget=env_robotModel.RobotModelWidget(self.rootObject)
        self.ui_Main.modelLayout.addWidget(self.modelWidget)

        #   原始视频数据
        self.video=videoWidget.VideoWidget()
        self.ui_Main.videoLayout.addWidget(self.video)
        #   日志数据
        self.logEdit=logWidget.LogEdit()
        self.ui_Main.logLayout.addWidget(self.logEdit)
        #   状态栏
        self.AddStatusBar()
        #   按钮绑定
        self.BindButtons()
        #   界面显示
        self.ui_Main.show()



    def BindButtons(self):
        '''
        按钮绑定
        :return:
        '''
        #   顶部菜单栏
        self.ui_Main.calibAction.triggered.connect(self.rootObject.CalibSensors)
        self.ui_Main.actionNo_Filter.triggered.connect(lambda: self.rootObject.SetFilterLevel(0))
        self.ui_Main.actionMedian_Filter.triggered.connect(lambda: self.rootObject.SetFilterLevel(1))
        self.ui_Main.actionKalman_Filter.triggered.connect(lambda: self.rootObject.SetFilterLevel(2))
        self.ui_Main.actionManual.triggered.connect(self.ShowJCtrls)
        self.ui_Main.actionBCtrl.triggered.connect(self.ShowBCtrls)
        self.ui_Main.actionPctrl.triggered.connect(self.ShowPCtrls)
        #   按钮绑定（本类）
        self.ui_Main.saveLogBtn.clicked.connect(self.logEdit.SaveLog)
        self.ui_Main.sxBtn.clicked.connect(lambda: self.modelWidget.SetView('x'))
        self.ui_Main.syBtn.clicked.connect(lambda: self.modelWidget.SetView('y'))
        self.ui_Main.szBtn.clicked.connect(lambda: self.modelWidget.SetView('z'))
        #   按钮绑定(关联到其他类)
        self.ui_Main.releaseBtn.clicked.connect(self.rootObject.ReleaseJoints)
        self.ui_Main.initjBtn.clicked.connect(None)
        self.ui_Main.zmplBtn.clicked.connect(lambda: self.rootObject.kinematic.StartPace())
        self.ui_Main.zmprBtn.clicked.connect(lambda: self.rootObject.kinematic.TransferSupport('right'))
        self.ui_Main.btn3.clicked.connect(lambda: self.rootObject.kinematic.Lift('right'))
        self.ui_Main.btn4.clicked.connect(lambda: self.rootObject.kinematic.Lift('left'))
        self.ui_Main.btn5.clicked.connect(lambda: print(self.rootObject.netBridge.GetJointVals()))
        self.ui_Main.initBtn_3.clicked.connect(lambda: self.rootObject.kinematic.Higher())
        #   录制按钮
        self.ui_Main.recordBtn.clicked.connect(self.StartRecord)
        self.ui_Main.stoprecordBtn.clicked.connect(self.SaveRecord)
    def AddStatusBar(self):
        '''
        状态栏信息
        :return:
        '''
        self.recStatusLabel = QLabel()
        self.recStatusLabel.setText('<font color=\"green\"> 未开始录制 </font>')
        self.ctrlStatusLabel = QLabel()
        self.ctrlStatusLabel.setText('静止平衡状态')
        self.latencyLabel=QLabel()
        self.latencyLabel.setText("网路延时:<font color=\"red\">未连接</font>")
        self.batteryLabel=battery.BatteryWidget()
        self.statusBar=QStatusBar()
        self.statusBar.addPermanentWidget(self.recStatusLabel, stretch=2)
        self.statusBar.addPermanentWidget(self.ctrlStatusLabel, stretch=2)
        self.statusBar.addPermanentWidget(self.latencyLabel,stretch=2)
        self.statusBar.addPermanentWidget(self.batteryLabel,stretch=1)
        self.ui_Main.setStatusBar(self.statusBar)

    ''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
    按钮绑定函数
    '''''
    def ShowJCtrls(self):
        '''
        显示关节控制界面
        :return:
        '''
        self.jCtrl=jctrl_panel.JPanel(rootObject=self.rootObject)
        self.jCtrl.ui_Main.show()
    def ShowBCtrls(self):
        self.bCtrl=bctrl_panel.BPanel(rootObject=self.rootObject)
        self.bCtrl.ui_Main.show()

    def ShowPCtrls(self):
        self.pCtrl=params_panel.PPanel(rootObject=self.rootObject)
        self.pCtrl.ui_Main.show()

    def StartRecord(self,maxSize=50000):
        '''
        :param maxSize:
        :return:
        '''
        interval = 1.0/self.rootObject.config['freq_high']
        maxSize = self.rootObject.config['max_record_size']
        self.t = 0.0
        self.maxTime=maxSize*interval
        self.recInterval=interval
        self.recorder=DataRecorder()
        self.timer_Rec = QTimer()
        self.timer_Rec.timeout.connect(self.FeedRecord)
        self.timer_Rec.start(int(1000 * interval))
        self.ui_Main.recordBar.setMaximum(int(maxSize*interval))
        self.rootObject.Log('开始记录数据')
        self.ui_Main.stoprecordBtn.setEnabled(True)
        self.ui_Main.recordBtn.setEnabled(False)
    def FeedRecord(self):
        self.recorder.FeedRot(self.t,self.rootObject.base.rotationMonitored,self.rootObject.lFoot.rotationMonitored,self.rootObject.rFoot.rotationMonitored)
        self.recorder.FeedGyro(self.t,self.rootObject.base.GetGyroRaw(),self.rootObject.lFoot.GetGyroRaw(),self.rootObject.rFoot.GetGyroRaw())
        self.recorder.FeedAccel(self.t,self.rootObject.base.GetAccelRaw(),self.rootObject.lFoot.GetAccelRaw(),self.rootObject.rFoot.GetAccelRaw())
        self.recorder.FeedPressure(self.t,self.rootObject.lFoot.pressure,self.rootObject.rFoot.pressure)
        self.recorder.FeedZMP(self.t,self.rootObject.lFoot.pressCenter,self.rootObject.rFoot.pressCenter)
        self.recorder.FeedJRaw(self.t,self.rootObject.lChain.joints,self.rootObject.rChain.joints)
        self.recorder.FeedJMod(self.t,self.rootObject.lChain.jErrorDynamic, self.rootObject.rChain.jErrorDynamic)
        self.recStatusLabel.setText('记录中\t{:.2f}K'.format(self.t/self.recInterval))
        self.ui_Main.recordBar.setValue(int(self.t))
        self.t += self.recInterval
        if self.t>=self.maxTime-1:
            self.recStatusLabel.setText('数据满，自动停止\t数据量{:.2f}K'.format(self.t / self.recInterval/1000))
            self.SaveRecord()
    def SaveRecord(self):
        self.timer_Rec.stop()
        self.recorder.Save()
        self.ui_Main.stoprecordBtn.setEnabled(False)
        self.ui_Main.recordBtn.setEnabled(True)
        self.rootObject.Log('录制数据存储到'+self.recorder.fileName)
        self.recStatusLabel.setText('记录完成，数据量\t{:.2f}K'.format(self.t / self.recInterval/1000))
if __name__=='__main__':
    app = QApplication([])
    MainActivity=Panel()
    app.exec_()